using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;

namespace Project10
{
    /// <summary>
    /// This is the main type for your game
    /// </summary>
    public class Game1 : Microsoft.Xna.Framework.Game
    {
        GraphicsDeviceManager graphics;
        SpriteBatch spriteBatch;
        SpriteFont spriteFont;
        InputListener inputListener;

        private Texture2D xAxis, yAxis;
        private Vector2 offSet;
        private string infoString;

        private Object ball1, ball2;
        private float e;
        private int collCount;

        private Vector2 N;
        private Vector2 T;
        private Vector2 velocity1;
        private Vector2 velocity2;
        private Vector2 valueN;
        private Vector2 jn;

        private float timer;
        private bool isStart;

        public Game1()
        {
            graphics = new GraphicsDeviceManager(this);
            Content.RootDirectory = "Content";
            this.IsMouseVisible = true;
            graphics.PreferredBackBufferWidth = 1280;
            graphics.PreferredBackBufferHeight = 720;
        }

        /// <summary>
        /// Allows the game to perform any initialization it needs to before starting to run.
        /// This is where it can query for any required services and load any non-graphic
        /// related content.  Calling base.Initialize will enumerate through any components
        /// and initialize them as well.
        /// </summary>
        protected override void Initialize()
        {
            // TODO: Add your initialization logic here
            inputListener = new InputListener(this);
            Components.Add(inputListener);

            xAxis = CreateRectangle(800, 1, Color.White);
            yAxis = CreateRectangle(800, 1, Color.White);
            offSet = new Vector2(400, 0);

            //Physics data
            e = 0.0f;
            collCount = 0;

            timer = 0.0f;
            isStart = false;

            base.Initialize();
        }

        /// <summary>
        /// LoadContent will be called once per game and is the place to load
        /// all of your content.
        /// </summary>
        protected override void LoadContent()
        {
            // Create a new SpriteBatch, which can be used to draw textures.
            spriteBatch = new SpriteBatch(GraphicsDevice);
            spriteFont = Content.Load<SpriteFont>("gameFont");

            // TODO: use this.Content to load your game content here
            Texture2D ballTexture1 = Content.Load<Texture2D>("ball1");
            Texture2D ballTexture2 = Content.Load<Texture2D>("ball2");

            ball1 = new Object(ballTexture1, new Vector2(-380, 370), 1.0f, new Vector2(100, 0));
            ball2 = new Object(ballTexture2, new Vector2(80, 400), 1.0f, Vector2.Zero);
            N = new Vector2(ball1.PositionX - ball2.PositionX, ball1.PositionY - ball2.PositionY);

            T = Vector2.Zero;
        }

        /// <summary>
        /// Allows the game to run logic such as updating the world,
        /// checking for collisions, gathering input, and playing audio.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Update(GameTime gameTime)
        {
            // Allows the game to exit
            if (inputListener.isKeyDown(Keys.Escape))
                this.Exit();

            // TODO: Add your update logic here
            if (inputListener.isKeyPressed(Keys.Space))
                isStart = !isStart;
            if (inputListener.isKeyPressed(Keys.Back))
            {
                Initialize();
            }

            if (!isStart && timer == 0.0f)
            {
                ControlData();
                N = new Vector2(ball2.PositionX - ball1.PositionX, ball2.PositionY - ball1.PositionY);
                velocity1 = ball1.InitialVelocity;
                velocity2 = ball2.InitialVelocity;
            }

            if (isStart)
            {
                timer += (float)gameTime.ElapsedGameTime.TotalSeconds;
                

                if (Vector2.Distance(new Vector2(ball1.PositionX, ball1.PositionY), new Vector2(ball2.PositionX, ball2.PositionY)) <= 40)
                {
                    ball1.PositionX = ball2.PositionX - (float)Math.Sqrt(Math.Pow(40, 2) - Math.Pow(ball2.PositionY - ball1.PositionY, 2));

                    N = new Vector2(ball2.PositionX - ball1.PositionX, ball2.PositionY - ball1.PositionY);
                    valueN = N;
                    N.Normalize();

                    Vector2 Uin = Vector2.Dot(velocity1, N) * N;
                    Vector2 Vin = Vector2.Dot(velocity2, N) * N;

                    Vector2 Vrn = Uin - Vin;
                    jn = -Vrn * (e + 1) * ball1.Mass * ball2.Mass / (ball1.Mass + ball2.Mass);

                    Vector2 Ufn = jn / ball1.Mass + Uin;
                    Vector2 Vfn = -jn / ball2.Mass + Vin;

                    Vector3 cross = Vector3.Cross(Vector3.Cross(new Vector3(N, 0), new Vector3(velocity1, 0)), new Vector3(N, 0));
                    T = new Vector2(cross.X, cross.Y);

                    if (T != Vector2.Zero)
                        T.Normalize();
                    else
                        T = Vector2.Zero;

                    Vector2 Uit = Vector2.Dot(velocity1, T) * T;
                    Vector2 Vit = Vector2.Dot(velocity2, T) * T;
                                  
                    velocity1 = Ufn + Uit;
                    velocity2 = Vfn + Vit;

                    ball1.FinalVelocity = Ufn + Uit;
                    ball2.FinalVelocity = Vfn + Vit;

                    collCount = 1;
                }

                ball1.PositionX += velocity1.X * (float)gameTime.ElapsedGameTime.TotalSeconds;
                ball1.PositionY += velocity1.Y * (float)gameTime.ElapsedGameTime.TotalSeconds;
                ball2.PositionX += velocity2.X * (float)gameTime.ElapsedGameTime.TotalSeconds;
                ball2.PositionY += velocity2.Y * (float)gameTime.ElapsedGameTime.TotalSeconds;
            }

            float p_ix = ball1.Mass * ball1.InitialVelocity.X + ball2.Mass * ball2.InitialVelocity.X;
            float p_fx = ball1.Mass * ball1.FinalVelocity.X + ball2.Mass * ball2.FinalVelocity.X;
            float p_fy = ball1.Mass * ball1.FinalVelocity.Y + ball2.Mass * ball2.FinalVelocity.Y;

            float ke_i = ball1.Mass * ball1.InitialVelocity.LengthSquared() / 2 + ball2.Mass * ball2.InitialVelocity.LengthSquared() / 2;
            float ke_f = ball1.Mass * ball1.FinalVelocity.LengthSquared() / 2 + ball2.Mass * ball2.FinalVelocity.LengthSquared() / 2;

            //Info
            infoString = "M1:   " + ball1.Mass + " kg    (" + ball1.PositionX.ToString("0.0") + " , " + ball1.PositionY.ToString("0.0") + ") m";
            infoString += "\nM2:  " + ball2.Mass + " kg    (" + ball2.PositionX.ToString("0.0") + " , " + ball2.PositionY.ToString("0.0") + ") m";
            infoString += "\n\nInitial:  u = (" + ball1.InitialVelocity.X + ", " + ball1.InitialVelocity.Y + ")" +
                          "   v = (" + ball2.InitialVelocity.X + " ," + ball2.InitialVelocity.Y + ") m/s";
            infoString += "\nFinal:  u = (" + ball1.FinalVelocity.X.ToString("0.0") + " , " + (-ball1.FinalVelocity.Y).ToString("0.0") + ")" + 
                          "   v = (" + ball2.FinalVelocity.X.ToString("0.0") + " , " + (-ball2.FinalVelocity.Y).ToString("0.0") + ") m/s";
            infoString += "\n\n e = " + e.ToString("0.0") + "    Collision = " + collCount;
            infoString += "\nImpulse J = " + (-jn.Length()).ToString("0.0") + " kg m/s";
            infoString += "\nN = (" + valueN.X.ToString("0.0") + " , " + valueN.Y.ToString("0.0") + ")";
            infoString += "\n\np_ix = " + (ball1.Mass * ball1.InitialVelocity.X).ToString("0.0") + " + " + (ball2.Mass * ball2.InitialVelocity.X).ToString("0.0") + " = " + p_ix.ToString("0.0") + " kg m/s";
            infoString += "\np_fx = " + (ball1.Mass * ball1.FinalVelocity.X).ToString("0.0") + " + " + (ball2.Mass * ball2.FinalVelocity.X).ToString("0.0") + " = " + p_fx.ToString("0.0") + " kg m/s";
            infoString += "\np_fy = " + (ball1.Mass * (-ball1.FinalVelocity.Y)).ToString("0.0") + " + " + (ball2.Mass * (-ball2.FinalVelocity.Y)).ToString("0.0") + " = " + p_fy.ToString("0.0") + " kg m/s";
            infoString += "\n\nKE_i = " + (ball1.Mass * ball1.InitialVelocity.LengthSquared() / 2).ToString("0.0") + " + " + (ball2.Mass * ball2.InitialVelocity.LengthSquared() / 2).ToString("0.0") + " = " + ke_i.ToString("0.0") + " J";
            infoString += "\nKE_f = " + (ball1.Mass * ball1.FinalVelocity.LengthSquared() / 2).ToString("0.0") + " + " + (ball2.Mass * ball2.FinalVelocity.LengthSquared() / 2).ToString("0.0") + " = " + ke_f.ToString("0.0") + " J";

            infoString += "\n\nTimer: " + timer.ToString("0.00") + " s";

            base.Update(gameTime);
        }

        /// <summary>
        /// This is called when the game should draw itself.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Draw(GameTime gameTime)
        {
            GraphicsDevice.Clear(Color.CornflowerBlue);

            // TODO: Add your drawing code here
            spriteBatch.Begin();
            //Coordinate
            spriteBatch.Draw(xAxis, new Vector2(0, 400), null, Color.White, 0f, new Vector2(0, 1), 1f, SpriteEffects.None, 0f);
            spriteBatch.Draw(yAxis, new Vector2(400, 400), null, Color.White, MathHelper.PiOver2, new Vector2(400, 0), 1f, SpriteEffects.None, 0f);

            //Objects            
            ball1.Draw(spriteBatch, offSet);
            ball2.Draw(spriteBatch, offSet);


            spriteBatch.DrawString(spriteFont, infoString, new Vector2(840, 50), Color.White);
            spriteBatch.End();

            base.Draw(gameTime);
        }

        private Texture2D CreateRectangle(int width, int height, Color objColor)
        {
            Texture2D rectTexture = new Texture2D(GraphicsDevice, width, height, false, SurfaceFormat.Color);

            Color[] color = new Color[width * height];

            for (int i = 0; i < color.Length; i++)
                color[i] = objColor;

            rectTexture.SetData(color);

            return rectTexture;
        }

        private void ControlData()
        {
            // Ball 1
            if (inputListener.isKeyPressed(Keys.Q))
                ball1.Mass += 1.0f;
            if (inputListener.isKeyPressed(Keys.A))
                ball1.Mass -= 1.0f;
            ball1.Mass = MathHelper.Clamp(ball1.Mass, 1.0f, 10.0f);

            if (inputListener.isKeyPressed(Keys.W) && ball1.InitialVelocity.X < 150)
                ball1.InitialVelocity += new Vector2(10.0f, 0);
            if (inputListener.isKeyPressed(Keys.S) && ball1.InitialVelocity.X > 50)
                ball1.InitialVelocity -= new Vector2(10.0f, 0);

            if (inputListener.isKeyPressed(Keys.Up))
                ball1.PositionY -= 10.0f;
            if (inputListener.isKeyPressed(Keys.Down))
                ball1.PositionY += 10.0f;
            ball1.PositionY = MathHelper.Clamp(ball1.PositionY, 370.0f, 430.0f);

            // Ball 2
            if (inputListener.isKeyPressed(Keys.U))
                ball2.Mass += 1.0f;
            if (inputListener.isKeyPressed(Keys.J))
                ball2.Mass -= 1.0f;
            ball2.Mass = MathHelper.Clamp(ball2.Mass, 1.0f, 10.0f);

            if (inputListener.isKeyPressed(Keys.I) && ball2.InitialVelocity.X < 10)
                ball2.InitialVelocity += new Vector2(10.0f, 0);
            if (inputListener.isKeyPressed(Keys.K) && ball2.InitialVelocity.X > -50)
                ball2.InitialVelocity -= new Vector2(10.0f, 0);

            if (inputListener.isKeyPressed(Keys.F))
                e += 0.1f;
            if (inputListener.isKeyPressed(Keys.G))
                e -= 0.1f;
            e = MathHelper.Clamp(e, 0.0f, 2.0f);
        }
    }
}
